3D relative pose estimation from six distances

In this paper, we present three fast, hybrid numericalgebraic methods to solve polynomial systems in floating point representation, based on the eigendecomposition of a so-called multiplication matrix. In particular, these methods run using standard double precision, use only linear algebra packages, and are easy to implement. We provide the proof that these methods do indeed produce valid multiplication matrices, and show their relationship. As a specific application, we use our algorithms to compute the 3D relative translation and orientation between two robots, based on known egomotion and six robotto-robot distance measurements. Equivalently, the same system of equations arises when solving the forward kinematics of the general Stewart-Gough mechanism. Our methods can find all 40 solutions, trading off speed (0.08s to 1.5s, depending on the choice of method) for accuracy.